#include "EventType.hpp"
#include "FollowColorLaneState.hpp"
#include "Hardware.hpp"
#include "Robot.hpp"

Robot& Robot::getInstance()
{
	//Iteratie 1
//	static Robot robot(new FreestyleState());
	//Iteratie 2
	static Robot robot(new FollowColorLaneState());
	return robot;
}

Robot::Robot(State* aStartState) :
	currentState(aStartState),
	sensorPoller()
{

}

void Robot::handleEvent(Event& event)
{
	switch (event.getEventType())
	{
		case EMERGENCYSTOPACTIVATED:
		{
			//handle this event:
			handleEmergencyStopActivatedEvent();
			break;
		}
		default: //Everything else
		{
			currentState->handleEvent(event); //Let the current state handle this event
			break;
		}
	}
}

void Robot::handleEmergencyStopActivatedEvent()
{
//	//Check if current state is EmergencyStopState
//	if (currentState.getStateType() != State::EMERGENCYSTOP)
//	{ //Current state is not EMERGENCYSTOP
//		//create new emergency state
//		EmergencyStopState newState;
//		changeState(newState);
//	}
}

void Robot::changeState(State* newState)
{
	delete currentState;
	//Change currentState
	currentState = newState;
	//Initialize new state
	currentState->startState();
}

State* Robot::getState()
{
	return currentState;
}

Engine& Robot::getEngine()
{
	return engine;
}

void Robot::init()
{
	//Init sensors:
	//TODO: Init sensors here

	//Post check:
	//TODO: POST Check here

	//Start robot:
	currentState->startState();
}

void Robot::restart()
{
	//TODO: keep polling sensors
	while (true)
	{
		sensorPoller.poll();
//		if (currentState.getStateType() == State::FREESTYLE)
//			currentState.initState();
		//Line below is necessary for simulator-runs in order to prevent live-locks
//		Hardware::getHardwareController().wait(10);
	}
}

void Robot::update()
{
	sensorPoller.poll();
	Hardware::getHardwareController().wait(10);
}
